%% Init
clc;close all;clear;
addpath("./M_Function/")

%% Settings

opts=bodeoptions;

% 设置bode的显示配置
% opts.FreqUnits='Hz'
opts.xlim=[0.01 1000]*2*pi;
opts.PhaseVisible = 'on';
opts.Grid= 'off';
opts.PhaseMatching = 'on'
opts.PhaseMatchingFreq = 100;
opts.PhaseMatchingValue = -18
opts.PhaseWrapping = 'on'
opts.Title.FontSize = 12;
opts.XLabel.FontSize = 12;
opts.YLabel.FontSize = 12;
opts.TickLabel.FontSize = 10;

% 设置plot的默认窗口大小
set(groot, 'DefaultFigurePosition',[180,50,2400,1000]);

%% Step1: Plant
s = tf('s');

Motor_w = 1000;
Motor_T = 1 / Motor_w;
Motor_tf = 1/(Motor_T*s+1)

freq_rads = (0.01:0.01:1000)*2*pi;
fs_rads = freq_rads(end)*s;
ts = 2*pi/fs_rads

%% Step2: The influence of wi on Sensitivity
w = 50
A = [1/10 1/5 1/2 1/1.1]
alpha = A 

% Param
K = 1 % 增益
T = 1./w % 时间常数
Tz = T./A % 零点对应时间常数
Tp = T % 报点对应时问需数
wz = 1./Tz % 零点对应转拆频率
wp = 1./Tp % 极点对应转折频率
wl = wz % 低位转折频率
wh = wp % 高位转折频丰
wm = 1./(T*sqrt(1./alpha)) % 相位极值点对应频率
phim_deg = rad2deg(asin((1-alpha)./(1+alpha))) % 最大提供相位

len = length(A)

% 不同参数下各函数响应
figure(21)
sgtitle("BoostDown" + ...
    "环节不同A下各函数响应" + " ( A=" + num2str(A) + ", w=" + num2str(w) + ' )')
legend_str = string([len,1]);
for i=1:len
    % Boost_tf(i)= K*(Tz*s+1)/(Tp(i)*s+1);
    Boost_tf(i)=K*(s+w*A(i))/(s+w);
    Ds_tf(i) = Boost_tf(i);
    Yol_tf(i) = Ds_tf(i) * Motor_tf;
    Ycl_tf(i) = Yol_tf(i) / (1 + Yol_tf(i));
    Scl_tf(i) = 1 / (1 + Yol_tf(i));

    legend_str(i) = "w1(wz)=" + num2str(wl(i)) + "  " + "wh(wp)=" + num2str(wh);
end

%% plot
subplot(1,4,1)
for i=1:len
    bode(Ds_tf(i), opts)
    hold on
end
legend(legend_str,'Location','best')
title("控制器传递函数Ds")


subplot(1,4,2)
for i=1:len
    bode(Yol_tf(i), opts)
    hold on
end
legend(legend_str,'Location','best')
title("开环传递函数Yol")


subplot(1,4,3)
for i=1:len
    bode(Ycl_tf(i), opts)
    hold on
end
legend(legend_str,'Location','best')
title("闭环传递函数Ycl")

subplot(1,4,4)
for i=1:len
    bode(Scl_tf(i), opts)
    hold on
end
legend(legend_str,'Location','best')
title("灵敏度函数Scl")
